function [ant, EvaluationNum, GenerationNum, OptFound] = lamsaco_test_countifnotclose(Problem, Algorithm, OptSet)
%LAMSACO Summary of this function goes here
%   Detailed explanation goes here
% PROBLEM SETTING
eta=1E-10;
d = Problem.Dimension;
LB = Problem.LowerBound;
UB = Problem.UpperBound;
ObjFunc = Problem.ObjFunc;
% ALGORITHM ASSIGNMENT
AntSize = Algorithm.AntSize;
NichSizeSet = Algorithm.NichSizeSet;
std_ls = Algorithm.std_ls;
N_ls = Algorithm.N_ls;
N = Algorithm.MaxFEs;
OptFound = [];
%% INITIALIZATION
GenerationNum = 1;  % the current generation number
ant = rand([AntSize,d]).*repmat((UB-LB),[AntSize,1])+repmat(LB,[AntSize,1]);
fitness = ObjFunc(ant);
EvaluationNum = AntSize;  % the budget used
maxF = max(fitness);
minF = min(fitness);
ant = [fitness,ant];  % [fitnesses, solution]
find_opt(ant);
%% OPTIMIZATION
while EvaluationNum<N && size(OptFound,1)<size(OptSet.sol,1)
    NS = NichSizeSet(ceil(rand().*length(NichSizeSet)));
    ClusterNum = ceil(AntSize/NS);
    species = cluster_speciation(ant,NS,ClusterNum);
    new_species = generate_ant(species,NS,ClusterNum);
    new_species(:,1) = ObjFunc(new_species(:,2:end));
    EvaluationNum = EvaluationNum+AntSize;
    species = replace_species(new_species,species,NS,ClusterNum);
    ant = species_ls(species,NS,ClusterNum);
    maxF = max(ant(:,1));
    minF = min(ant(:,1));
    GenerationNum = GenerationNum+1;
    find_opt(ant);
end
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function species = cluster_speciation(ant,NS,ClusterNum)
        species = zeros(AntSize,d+1);
        [~,ant_id] = sort(ant(:,1),'descend');
        ant = ant(ant_id,:);
        distance = zeros(AntSize, AntSize);
        for ii = 1:AntSize
            distance(ii,:) = sqrt(sum((repmat(ant(ii,2:end),[AntSize,1]) - ant(:,2:end)).^2,2));
        end
        id_finished = 0;
        for ii = 1:ClusterNum
            [~,id_best] = max(ant(:,1));
            [~, sol_id0] = sort(distance(id_best,:));
            id_start = id_finished+1;
            id_finished = min([id_finished+NS,AntSize]);
            sol_id = sol_id0(1:(id_finished-id_start+1));
            species(id_start:id_finished,:) = ant(sol_id,:);
            ant(sol_id,1) = -inf;
            distance(sol_id,:) = inf;
            distance(:,sol_id) = inf;
        end
    end
    function new_species = generate_ant(species,NS,ClusterNum)
        new_species = zeros(AntSize,d+1);
        id_finished = 0;
        for ii = 1:ClusterNum
            id_start = id_finished+1;
            id_finished = min([id_finished+NS,AntSize]);
            id_num = id_finished-id_start+1;
            [~,sol_id] = sort(species(id_start:id_finished,1),'descend');
            species(id_start:id_finished,:) = species(id_start+sol_id-1,:);
            maxFi = max(species(id_start:id_finished,1));
            minFi = min(species(id_start:id_finished,1));
            sigma_i = 0.1+0.3*exp(-(maxFi-minFi)/(maxF-minF+eta));
            w = 1/sigma_i/id_num/sqrt(2*pi).*exp(-(0:(id_num-1)).^2./(2*(sigma_i^2)*(id_num^2)));
            p = w./sum(w);
            cum_p = cumsum(p);
            id_new = 0;
            for jj = 1:id_num
                u = rand();
                id = find(cum_p>=u,1);
                x = species(id_start+id-1,2:end);
                if rand()<=0.5
                    x0 = x;
                else
                    F = rand([1,d]);
                    x0 = x+F.*(species(id_start,2:end)-x);
                end
                epsilon = rand();
                delta_d = epsilon/(id_num-1).*sum(abs(species(id_start:id_finished,2:end)-repmat(x,[id_num,1])),1);
                new_ant = normrnd(x0,delta_d);
                resample_id = (new_ant<LB)+(new_ant>UB);
                while sum(resample_id)>0
                    new_ant(resample_id==1) = normrnd(x0(resample_id==1),delta_d(resample_id==1));
                    resample_id = (new_ant<LB)+(new_ant>UB);
                end
                new_species(id_start+id_new,2:end) = new_ant;
                id_new = id_new+1;
            end
        end
    end
    function new_species = generate_ant_v2(species,NS,ClusterNum)
        new_species = zeros(AntSize,d+1);
        id_finished = 0;
        [~,sol_id] = sort(species(:,1),'descend');
        ranking = zeros(AntSize,1);
        ranking(sol_id) = (1:AntSize)';
        for ii = 1:ClusterNum
            id_start = id_finished+1;
            id_finished = min([id_finished+NS,AntSize]);
            id_num = id_finished-id_start+1;
            [~,sol_id] = sort(species(id_start:id_finished,1),'descend');
            species(id_start:id_finished,:) = species(id_start+sol_id-1,:);
            ranking(id_start:id_finished,:) = ranking(id_start+sol_id-1,:);
            maxFi = max(species(id_start:id_finished,1));
            minFi = min(species(id_start:id_finished,1));
            sigma_i = 0.1+0.3*exp(-(maxFi-minFi)/(maxF-minF+eta));
            w = 1/sigma_i/AntSize/sqrt(2*pi).*exp(-(ranking(id_start:id_finished)-1).^2./(2*(sigma_i^2)*(AntSize^2)));
            p = w./sum(w);
            cum_p = cumsum(p);
            id_new = 0;
            for jj = 1:id_num
                u = rand();
                id = find(cum_p>=u,1);
                x = species(id_start+id-1,2:end);
                if rand()<=0.5
                    x0 = x;
                else
                    F = rand();
                    x0 = x+F.*(species(id_start,2:end)-x);
                end
                epsilon = rand();
                delta_d = epsilon/(id_num-1).*sum(abs(species(id_start:id_finished,2:end)-repmat(x,[id_num,1])),1);
                new_ant = normrnd(x0,delta_d);
                resample_id = (new_ant<LB)+(new_ant>UB);
                while sum(resample_id)>0
                    new_ant(resample_id==1) = normrnd(x0(resample_id==1),delta_d(resample_id==1));
                    resample_id = (new_ant<LB)+(new_ant>UB);
                end
                new_species(id_start+id_new,2:end) = new_ant;
                id_new = id_new+1;
            end
        end
    end
    function species = replace_species(new_species,species,NS,ClusterNum)
        id_finished = 0;
        for ii = 1:ClusterNum
            id_start = id_finished+1;
            id_finished = min([id_finished+NS,AntSize]);
            id_num = id_finished-id_start+1;
            for jj = id_start:id_finished
                distance = sum((species(id_start:id_finished,2:end)-repmat(new_species(jj,2:end),[id_num,1])).^2,2);
                [~,id] = min(distance);
                if species(id_start+id-1,1)<new_species(jj,1)
                    species(id_start+id-1,:) = new_species(jj,:);
                end
            end
        end
    end
    function species = species_ls(species,NS,ClusterNum)
        id_finished = 0;
        seeds = zeros(ClusterNum,1);
        for ii = 1:ClusterNum
            id_start = id_finished+1;
            id_finished = min([id_finished+NS,AntSize]);
            [~,id] = max(species(id_start:id_finished,1));
            seeds(ii) = id_start+id-1;
        end
        maxFSE = max(species(seeds,1));
        minFSE = min(species(seeds,1));
        flag = 0;
        if minFSE<=0
            maxFSE = maxFSE+abs(minFSE)+eta;
            flag = 1;
        end
        pro = zeros(ClusterNum,1);
        for ii = 1:ClusterNum
            if flag == 1
                pro(ii) = (species(seeds(ii),1)+abs(minFSE)+eta)/(maxFSE+abs(minFSE)+eta);
            else
                pro(ii) = species(seeds(ii),1)/maxFSE;
            end
            uu = rand();
            if uu<=pro(ii)
                for jj = 1:N_ls
                    x0 = species(seeds(ii),2:end);
                    new_indivial = normrnd(x0,std_ls);
                    resample_id = (new_indivial<LB)+(new_indivial>UB);
                    while sum(resample_id)>0
                        new_indivial(resample_id==1) = normrnd(x0(resample_id==1),std_ls);
                        resample_id = (new_indivial<LB)+(new_indivial>UB);
                    end
                    f_value = ObjFunc(new_indivial);
                    EvaluationNum = EvaluationNum+1;
                    if f_value>species(seeds(ii),1)
                        species(seeds(ii),:) = [f_value,new_indivial];
                    end
                end
            end
        end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function find_opt( new_samples )
    % remove_opt: remove the found optimal solution from OptSetRemain
        new_samples(new_samples(:,1)<=(OptSet.fval-OptSet.epsilon),:) = [];
        if isempty(OptFound) && ~isempty(new_samples)
            OptFound = (new_samples(1,2:end));
            new_samples(1,:) = [];
        end
        for ii = 1:size(new_samples,1)
            OptDis = sqrt(sum((repmat(new_samples(ii,2:end),[size(OptFound,1),1]) - OptFound).^2,2));
            MinDis = min(OptDis);
            if MinDis>OptSet.radius
                OptFound = [OptFound;new_samples(ii,2:end)];
            end
        end
    end
end

